#include "CircleMass.h"
using namespace ms;

CircleMass::CircleMass(std::list<CircleMass> * masterListPtr_, real x_, real y_, real radius_, real mass_, real vx_, real vy_) {
	masterListPtr = masterListPtr_;
	position.x = x_;
	position.y = y_;
	lastPosition.set(&position);
	mass = mass_;
	velocity.x = vx_;
	velocity.y = vy_;
	radius = radius_;
}

Vector2D CircleMass::getVelocity() {
	return velocity;
}

real CircleMass::distSqr (Vector2D v1, Vector2D v2) {
	return real_pow((v2.x - v1.x), 2) + real_pow((v2.y - v1.y), 2);
}

void CircleMass::updateTerms(real timeScale) {
	/*
	 * TODO: Consider sharing these calculations with the other masse, so it cuts calculations in half
	 * F = (GMm)/(r^2)
	 * Fx = F * cos(t)
	 * Fy = F * sin(t)
	 */

	sumOfForces.set(0, 0);
	real tmpForce = 0; // magnitude of force between this and the other mass
	real tmpDistSq = 0; // distance^2 between this and the other mass
	real t = 0; // The angle between this and the other mass
	std::list<CircleMass>::iterator i; // used to iterate through masterList

	for (i = masterListPtr -> begin(); i != masterListPtr -> end(); ++i) { // iterate through each mass
		tmpDistSq = distSqr(position, i -> position);
		if (tmpDistSq != 0) { // don't count your self or /0
			tmpForce = (GRAV * mass * (i -> mass)) / (tmpDistSq); // Find the attractive force between the masses
			// tan(t) = (dy) / (dx)
			t = atan2(((i -> position.y) - position.y), ((i -> position.x) - position.x));
			sumOfForces.add(tmpForce * cos(t), tmpForce * sin(t)); // add the split-up force to a vector

			// F = m * a
			// a = F / m
			// v = vi + a * t

			velocity.x += (sumOfForces.x / mass) * timeScale;
			velocity.y += (sumOfForces.y / mass) * timeScale;
		}
	}
}

void CircleMass::updateLocation(real timeScale, bool collision) {
	lastPosition.set(&position);
	position.x += velocity.x * timeScale;
	position.y += velocity.y * timeScale;
	if (collision) { // only solve for collision if it is enabled
		solveForCollision();
	}
}

void CircleMass::solveForCollision() {
	// TODO: current method causes serious problems when one mass is trapped between two other masses (they can't escape), as well as other problems w/ >2 masses
	// TODO: doesn't work for really fast moving objects (that pass through each other)
	// TODO: make sure the user doesn't add any objects inside of each other (nothing would happen)
	std::list<CircleMass>::iterator i; // used to iterate through masterList
	real tmpDist;
	real sumOfRadii;
	Vector2D tmpP; // used for temporary storage of the other mass' momentum
	velocityBeforeCollision.set(&velocity);
	for (i = masterListPtr -> begin(); i != masterListPtr -> end(); ++i) { // iterate through each massterList
		tmpDist = real_sqrt(distSqr(position, i -> position)); // distance between this and the other mass
		sumOfRadii = radius + i -> radius; // maximum distance without collision
		if (sumOfRadii > tmpDist && tmpDist != 0) { // if the objects are inside of each other and they are different objects
			/* p = mv
			 * sum(pi) == sum(pf)
			 * p = CircleMass.mass * CircleMass.velocity
			 * this.velocity = p / this.mass
			 */

			// calculate and apply momentum for *this* object
			position.set(&lastPosition);
			tmpP.set(&(i -> velocityBeforeCollision));
			tmpP.mult(i -> mass); 
			tmpP.mult(PERCMOMENTUMREMAINING); // account for friction and error
			tmpP.div(mass);
			velocity.set(&tmpP); // set the velocity to the momentum / mass


			// calculate and apply momentum for the other object in collision
			i -> position.set(&(i -> lastPosition));
			tmpP.set(&velocityBeforeCollision);
			tmpP.mult(mass);
			tmpP.mult(PERCMOMENTUMREMAINING); // account for friction and error
			tmpP.div(i -> mass);
			i -> velocity.set(&tmpP);
		}
	}
}
